﻿#include "body.h"
#include "soft_render.h"

SoftRender::SoftRender(const Intrinsics& intrinsics)
  : buff(intrinsics.width, intrinsics.height)
{
  intrinsics_ = intrinsics;

  ClearNormalImage();
  ClearDepthImage();
  ClearSilhouetteImage();
}

bool SoftRender::AddBody(Body* body_ptr) {
  const std::lock_guard<std::mutex> lock{ mutex_ };

  if (nullptr == body_ptr) {
    return false;
  }

  bodies_.push_back(body_ptr);

  return true;
}

bool SoftRender::DeleteBody(Body* body_ptr) {
  return true;
}

void SoftRender::ClearBodies() {

}

bool SoftRender::RenderNormal() {
  return true;
}

Eigen::Vector3f world2screen(float x, float y, float z) {
  int focal = 300;
  int px = int(x * focal + 320 + 0.5f);
  int py = int(y * focal + 256 + 0.5f);
  return Eigen::Vector3f(px, py, z);
}

bool SoftRender::RenderSilhouette() {
  buff.Clear();
#if 0
  int cx = 320, cy = 240;
  Eigen::Vector2i v0(100 + cx, cy - 100);
  Eigen::Vector2i v1(-100 + cx, cy);
  Eigen::Vector2i v2(cx, 200 + cy);
  FillFace(v0, v1, v2, 255);
#else
  auto mesh = bodies_[0]->mesh();
  size_t n_face = mesh->indices.size();
  for (int i = 0; i < n_face; ++i) {
    Eigen::Vector3f screen_points[3];
    auto face = mesh->indices[i];
    for (int j = 0; j < 3; ++j) {
      auto vertex = mesh->vertices[face[j]];
      screen_points[j] = world2screen(vertex[0], vertex[1], 1.f);
    }
    FillFace(screen_points[0], screen_points[1], screen_points[2], 255);
  }
#endif
  return true;
}

float SoftRender::Depth(ushort depth_image_value) const {
  return 0;
}

float SoftRender::Depth(const cv::Point2i& image_coordinate) const {
  return 0;
}

Eigen::Vector3f SoftRender::PointVector(const cv::Point2i& image_coordinate) const {
  return Eigen::Vector3f(0, 0, 0);
}

bool SoftRender::FetchDepthImage() {
  const std::lock_guard<std::mutex> lock{ mutex_ };
  std::copy(
    buff.depth.begin(), buff.depth.end(),
    depth_image_.data);
  return true;
}

bool SoftRender::FetchNormalImage() {
  return true;
}

bool SoftRender::FetchSilhouetteImage() {
  const std::lock_guard<std::mutex> lock{ mutex_ };
  std::copy(
    buff.fill.begin(), buff.fill.end(), 
    silhouette_image_.data);
  return true;
}

template<typename T>
inline T CLAMP(T x, T a, T b) {
  return x < a ? a : x > b ? b : x;
}

#if 0
void SoftRender::FillFace(
  const Eigen::Vector3f& v1,
  const Eigen::Vector3f& v2,
  const Eigen::Vector3f& v3,
  unsigned char color) 
{
  Eigen::Vector3f A = v1, B = v2, C = v3;
  if (A.y() > B.y()) std::swap(A, B);
  if (A.y() > C.y()) std::swap(A, C);
  if (B.y() > C.y()) std::swap(B, C);

  auto BA = B - A;
  auto dx1 = BA.y() > 0 ? BA.x() / BA.y() : BA.x();
  auto dz1 = BA.y() != 0 ? BA.z() / BA.y() : 0;

  auto CA = C - A;
  auto dx2 = CA.y() > 0 ? CA.x() / CA.y() : 0;
  auto dz2 = CA.y() != 0 ? CA.z() / CA.y() : 0;

  auto CB = C - B;
  auto dx3 = CB.y() > 0 ? CB.x() / CB.y() : 0;
  auto dz3 = CB.y() != 0 ? CB.z() / CB.y() : 0;
  
  auto S = A, E = A;

  B.y() = floor(B.y() - 0.5f); 
  C.y() = floor(C.y() - 0.5f);

  float dxa, dxb, dza, dzb;

  if (dx1 > dx2) { 
    dxa = dx2; dxb = dx1; 
    dza = dz2; dzb = dz1; 
  } else { 
    dxa = dx1; dxb = dx2; 
    dza = dz1; dzb = dz2; 
  }

  while (S.y() <= B.y()) {
    auto dzX = (E.x() != S.x()) ? (E.z() - S.z()) / (E.x() - S.x()) : 0;
    auto Sz = S.z();

    auto Sy = CLAMP(S.y(), 0, (float)buff.height - 1);
    auto Sx = CLAMP(S.x(), 0, (float)buff.width - 1);
    auto Ex = CLAMP(E.x(), 0, (float)buff.width - 1);

    for (auto i = (size_t)Sx; i < Ex; i++) {
      int py = floor(Sy);
      auto index = PIXELMATINDEX(i, py, buff.width);
      if (Sz < buff.depth[index]) {
        buff.fill[index] = color;
        buff.depth[index] = Sz;
      }
      Sz += dzX;
    }

    S.y()++, E.y()++;
    S.x() += dxa, E.x() += dxb;
    S.z() += dza, E.z() += dzb;
  }

  if (dx1 > dx2) { 
    dxa = dx2; dxb = dx3; 
    dza = dz2; dzb = dz3; 
    E = B; 
  } else { 
    dxa = dx3; dxb = dx2; 
    dza = dz3; dzb = dz2; 
    S = B; 
  }

  while (S.y() <= C.y()) {
    auto dzX = (E.x() != S.x()) ? (E.z() - S.z()) / (E.x() - S.x()) : 0;
    auto Sz = S.z();

    auto Sy = CLAMP(S.y(), 0, (float)buff.height - 1);
    auto Sx = CLAMP(S.x(), 0, (float)buff.width - 1);
    auto Ex = CLAMP(E.x(), 0, (float)buff.width - 1);

    for (auto i = (size_t)Sx; i < Ex; i++) {
      auto index = PIXELMATINDEX(i, Sy, buff.width);
      if (Sz < buff.depth[index]) {
        buff.fill[index] = color;
        buff.depth[index] = Sz;
      }
      Sz += dzX;
    }

    S.y()++, E.y()++;
    S.x() += dxa, E.x() += dxb;
    S.z() += dza, E.z() += dzb;
  }
}
#endif

void SoftRender::FillFace(
  const Eigen::Vector3f& v1,
  const Eigen::Vector3f& v2,
  const Eigen::Vector3f& v3,
  unsigned char color)
{
  if (v1.y() == v2.y() && v1.y() == v3.y()) return;

  Eigen::Vector3f t0 = v1, t1 = v2, t2 = v3;
  if (t0.y() > t1.y()) std::swap(t0, t1);
  if (t0.y() > t2.y()) std::swap(t0, t2);
  if (t1.y() > t2.y()) std::swap(t1, t2);

  int width_minus_1 = buff.width - 1;
  int height_minus_1 = buff.height - 1;
  int sy = CLAMP(int(t0.y()), 0, height_minus_1);
  int ey = CLAMP(int(t2.y()), 0, height_minus_1);

  Eigen::Vector3f t20 = t2 - t0;
  Eigen::Vector3f t21 = t2 - t1;
  Eigen::Vector3f t10 = t1 - t0;
  float segment_height0 = t1.y() - t0.y();
  float segment_height1 = t2.y() - t1.y();
  float total_height = t2.y() - t0.y();

  for (int y = sy; y < ey; y++) {
    bool second_half = y > t1.y() || t1.y() == t0.y();

    float alpha = (y - t0.y()) / total_height;
    float beta = second_half ?
      (y - t1.y()) / segment_height1 :
      (y - t0.y()) / segment_height0;

    Eigen::Vector3f S = t0 + t20 * alpha;
    Eigen::Vector3f E = second_half ?
      t1 + t21 * beta :
      t0 + t10 * beta;

    if (S.x() > E.x()) std::swap(S, E);

    int Sx = CLAMP(int(S.x()), 0, width_minus_1);
    int Ex = CLAMP(int(E.x()), 0, width_minus_1);
    float SEx = E.x() - S.x();
    float SEz = E.z() - S.z();
    for (int x = Sx; x <= Ex; x++) {
      int index = x + y * buff.width;
#if 0
      buff.fill[index] = color;
#else
      float depth = SEz * (x - S.x()) / SEx;
      float& buff_depth = buff.depth[index];
      if (depth < buff_depth) {
        buff_depth = depth;
        buff.fill[index] = color;
      }
#endif
    }
  }
}